using BepuUtilities;
using BepuUtilities.Memory;
using System;
using System.Diagnostics;
using System.Runtime.CompilerServices;
using static BepuUtilities.GatherScatter;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif

namespace BepuPhysics.Constraints
{
    /// <summary>
    /// 将附着到两个实体的轴限制在最大摆动角度内。
    /// </summary>
    public struct SwingLimit : ITwoBodyConstraintDescription<SwingLimit>
    {
        /// <summary>
        /// 在主体A的局部空间中附着到主体A的轴。
        /// </summary>
        public Vector3 AxisLocalA;
        /// <summary>
        /// 在主体B的局部空间中附着到主体B的轴。
        /// </summary>
        public Vector3 AxisLocalB;
        /// <summary>
        /// 约束试图保持的世界空间A和B轴之间的最小点积。
        /// </summary>
        public float MinimumDot;
        /// <summary>
        /// 弹簧频率和阻尼参数。
        /// </summary>
        public SpringSettings SpringSettings;

        /// <summary>
        /// 基于MinimumDot字段获取或设置约束允许的世界轴A和B之间的最大摆动角度。
        /// </summary>
        public float MaximumSwingAngle { get { return (float)Math.Acos(MinimumDot); } set { MinimumDot = (float)Math.Cos(value); } }

        public int ConstraintTypeId
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                return SwingLimitTypeProcessor.BatchTypeId;
            }
        }

        public Type TypeProcessorType => typeof(SwingLimitTypeProcessor);

        public void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
        {
            ConstraintChecker.AssertUnitLength(AxisLocalA, nameof(SwingLimit), nameof(AxisLocalA));
            ConstraintChecker.AssertUnitLength(AxisLocalB, nameof(SwingLimit), nameof(AxisLocalB));
            Debug.Assert(MinimumDot >= -1f && MinimumDot <= 1f, "SwingLimit.MinimumDot must be from -1 to 1 inclusive.");
            ConstraintChecker.AssertValid(SpringSettings, nameof(SwingLimit));
            Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type.");
            ref var target = ref GetOffsetInstance(ref Buffer<SwingLimitPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
            GetFirst(ref target.AxisLocalA.X) = AxisLocalA.X;
            GetFirst(ref target.AxisLocalA.Y) = AxisLocalA.Y;
            GetFirst(ref target.AxisLocalA.Z) = AxisLocalA.Z;
            GetFirst(ref target.AxisLocalB.X) = AxisLocalB.X;
            GetFirst(ref target.AxisLocalB.Y) = AxisLocalB.Y;
            GetFirst(ref target.AxisLocalB.Z) = AxisLocalB.Z;
            GetFirst(ref target.MinimumDot) = MinimumDot;
            GetFirst(ref target.SpringSettings.AngularFrequency) = SpringSettings.AngularFrequency;
            GetFirst(ref target.SpringSettings.TwiceDampingRatio) = SpringSettings.TwiceDampingRatio;
        }

        public void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out SwingLimit description)
        {
            Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type.");
            ref var source = ref GetOffsetInstance(ref Buffer<SwingLimitPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
            description.AxisLocalA.X = GetFirst(ref source.AxisLocalA.X);
            description.AxisLocalA.Y = GetFirst(ref source.AxisLocalA.Y);
            description.AxisLocalA.Z = GetFirst(ref source.AxisLocalA.Z);
            description.AxisLocalB.X = GetFirst(ref source.AxisLocalB.X);
            description.AxisLocalB.Y = GetFirst(ref source.AxisLocalB.Y);
            description.AxisLocalB.Z = GetFirst(ref source.AxisLocalB.Z);
            description.MinimumDot = GetFirst(ref source.MinimumDot);
            description.SpringSettings.AngularFrequency = GetFirst(ref source.SpringSettings.AngularFrequency);
            description.SpringSettings.TwiceDampingRatio = GetFirst(ref source.SpringSettings.TwiceDampingRatio);
        }
    }

    public struct SwingLimitPrestepData
    {
        public Vector3Wide AxisLocalA;
        public Vector3Wide AxisLocalB;
        public Vector<float> MinimumDot;
        public SpringSettingsWide SpringSettings;
    }

    public struct SwingLimitProjection
    {
        // JacobianB=-JacobianA,所以不需要显式存储它。
        public Vector3Wide VelocityToImpulseA;
        public Vector<float> BiasImpulse;
        public Vector<float> SoftnessImpulseScale;
        public Vector3Wide ImpulseToVelocityA;
        public Vector3Wide NegatedImpulseToVelocityB;
    }

    public struct SwingLimitFunctions : IConstraintFunctions<SwingLimitPrestepData, SwingLimitProjection, Vector<float>>
    {
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB,
            ref SwingLimitPrestepData prestep, out SwingLimitProjection projection)
        {
            bodies.GatherOrientation(ref bodyReferences, count, out var orientationA, out var orientationB);

            // 摆动限制尝试将躯干A上的轴与躯干B上的轴保持在同一范围内。换言之,这与铰链关节相同,但自由度少了一个。
            // (请注意,Jacobian与AngularSwivelHinger极其相似;不同之处在于这是一个投机性的不等式约束。)
            // C=网点(Axisa,AxisB)>=最小网点
            // C‘=点(d/dt(Axisa),axisB)+点(Axisa,d/dt(AxisB))>=0
            // C‘=点(角度速度A x轴B)+点(轴A,角度速度B x轴B)>=0
            // C‘=点(Axisa x axisB,angularVelocityA)+点(angularVelocityB,axisB x Axisa)>=0
            // 提供以下方面的雅克比：
            // JA=Axisa x AxisB
            // JB=axisB x Axisa
            // a x b==-b x a,所以jb==。

            // 现在,我们选择存储表示法。默认方法是存储JA、有效质量和两个逆惯性,需要6+1+6+6标量。
            // 另一种方法是存储JAT*EffectiveMasses,然后存储JA*inverseInertia张量(A/B),这只需要3+3+3个标量。
            // 因此,总的来说,预焙为我们节省了10个标量和一些迭代时间的ALU。
            QuaternionWide.TransformWithoutOverlap(prestep.AxisLocalA, orientationA, out var axisA);
            QuaternionWide.TransformWithoutOverlap(prestep.AxisLocalB, orientationB, out var axisB);
            Vector3Wide.CrossWithoutOverlap(axisA, axisB, out var jacobianA);
            // 在轴平行的情况下,不存在唯一的雅可比。随便挑一个。
            // 请注意,这会导致两极的雅可比长度不连续。我们只是不担心这件事。
            Helpers.FindPerpendicular(axisA, out var fallbackJacobian);
            Vector3Wide.Dot(jacobianA, jacobianA, out var jacobianLengthSquared);
            var useFallback = Vector.LessThan(jacobianLengthSquared, new Vector<float>(1e-7f));
            Vector3Wide.ConditionalSelect(useFallback, fallbackJacobian, jacobianA, out jacobianA);

            // 注意,JA=-JB,但为了计算有效质量,符号是无关的。

            // 这使用通常的(J*M^-1*JT)^-1公式来计算有效质量,但是我们实际上利用了中间结果J*M^-1,所以我们直接计算它。
            Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaA.InverseInertiaTensor, out projection.ImpulseToVelocityA);
            // 注意,我们在这里没有使用-JacobianA,所以我们实际上存储了转换的否定版本。这很好;我们只需在迭代中减去即可。
            Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaB.InverseInertiaTensor, out projection.NegatedImpulseToVelocityB);
            Vector3Wide.Dot(projection.ImpulseToVelocityA, jacobianA, out var angularA);
            Vector3Wide.Dot(projection.NegatedImpulseToVelocityB, jacobianA, out var angularB);

            SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale);
            var effectiveMass = effectiveMassCFMScale / (angularA + angularB);
            Vector3Wide.Scale(jacobianA, effectiveMass, out projection.VelocityToImpulseA);

            Vector3Wide.Dot(axisA, axisB, out var axisDot);
            var error = axisDot - prestep.MinimumDot;
            // 注意否定：我们想要反对分离。TODO：可以说,应该将否定烘焙到PositionErrorToVelocity(给定其名称)。
            var biasVelocity = -Vector.Min(error * new Vector<float>(inverseDt), error * positionErrorToVelocity);
            projection.BiasImpulse = effectiveMass * biasVelocity;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static void ApplyImpulse(ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB, ref SwingLimitProjection projection, ref Vector<float> csi)
        {
            Vector3Wide.Scale(projection.ImpulseToVelocityA, csi, out var velocityChangeA);
            Vector3Wide.Add(angularVelocityA, velocityChangeA, out angularVelocityA);
            Vector3Wide.Scale(projection.NegatedImpulseToVelocityB, csi, out var negatedVelocityChangeB);
            Vector3Wide.Subtract(angularVelocityB, negatedVelocityChangeB, out angularVelocityB);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref SwingLimitProjection projection, ref Vector<float> accumulatedImpulse)
        {
            ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref accumulatedImpulse);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref SwingLimitProjection projection, ref Vector<float> accumulatedImpulse)
        {
            // JB=-JA。这是(angularVelocityA*JA+angularVelocityB*JB)*EffectiveMass=>(angularVelocityA-angularVelocityB)*(JA*EffectiveMass)
            Vector3Wide.Subtract(velocityA.Angular, velocityB.Angular, out var difference);
            Vector3Wide.Dot(difference, projection.VelocityToImpulseA, out var csi);
            // CSI=投射.BiasImPulse-累加脉冲*投射.SoftnessImPulseScale-(csia线性+csiaAngular+csib线性+csibAngular);
            csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - csi;

            var previousAccumulatedImpulse = accumulatedImpulse;
            accumulatedImpulse = Vector.Max(Vector<float>.Zero, accumulatedImpulse + csi);
            csi = accumulatedImpulse - previousAccumulatedImpulse;
            ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref csi);
        }

    }

    public class SwingLimitTypeProcessor : TwoBodyTypeProcessor<SwingLimitPrestepData, SwingLimitProjection, Vector<float>, SwingLimitFunctions>
    {
        public const int BatchTypeId = 25;
    }
}

